/**
 * @file:          Chopper.c
 * @brief:
 * @details:
 * @author:        wjh
 * @date created:  2023.07.13
 * @version:       1.0.0
 * @par copyright (c):
 *
 * @par history (desc):
 *   version:1.0.0, wjh, 2023.07.13,14:38:22
 */

/* Include Files **************************************************************/
#include "Chopper.h"
#include "common.h"

/* Global Variable Define *****************************************************/

/* Function Define ************************************************************/
void Chopper_Init(struct Chopper *self)
{
    self->OverModulation = Chopper_OverModulation;
    switch (self->param_svpwm_mode)
    {
    case SVPWMMode_2phase4vector:
        self->SVPWM = Chopper_SVPWM_2phase4vector;
        break;
    case SVPWMMode_2phase8vector:
        self->SVPWM = Chopper_SVPWM_2phase8vector;
        break;
    case SVPWMMode_3phase6vector:
        self->SVPWM = Chopper_SVPWM_3phase6vector;
        break;
    default:
        self->param_svpwm_mode = SVPWMMode_2phase8vector;
        self->SVPWM = Chopper_SVPWM_2phase8vector;
        break;
    }
    switch (self->param_chopper_mode)
    {
    case ChopperMode_FastDecay:
        self->Output = Chopper_Output_FastDecay;
        break;
    case ChopperMode_SlowDecay:
        self->Output = Chopper_Output_SlowDecay;
        break;
    case ChopperMode_HybridDecay:
        self->Output = Chopper_Output_HybridDecay;
        break;
    case ChopperMode_AdaptiveHybridDecay:
        // self->Output = Chopper_Output_AdaptiveHybridDecay;
        break;
    default:
        self->param_chopper_mode = ChopperMode_SlowDecay;
        self->Output = Chopper_Output_SlowDecay;
        break;
    }

    self->offset_pwm = self->param_offset_pwm_en != 0 ? self->param_offset_pwm : 0;
}

void Chopper_OverModulation(struct Chopper *self)
{
    /* Over modulation processing */
    float amp = bm_sqrt_carmack(self->in_Ualpha * self->in_Ualpha + self->in_Ubeta * self->in_Ubeta);

    if (amp > SQRT_2)
    {
        self->in_Ualpha = SQRT_2 * self->in_Ualpha / amp;
        self->in_Ubeta = SQRT_2 * self->in_Ubeta / amp;
    }
}

void Chopper_SVPWM_2phase4vector(struct Chopper *self)
{

    // self->OverModulation(self);
}

void Chopper_SVPWM_2phase8vector(struct Chopper *self)
{
    self->A = self->in_Ualpha > 0 ? 0 : 1;
    self->B = self->in_Ubeta > 0 ? 0 : 1;
    self->C = self->in_Ualpha - self->in_Ubeta > 0 ? 0 : 1;
    self->D = self->in_Ualpha + self->in_Ubeta > 0 ? 0 : 1;
    self->N = 8 * self->A + 4 * self->B + 2 * self->C + self->D;

    self->X = self->in_Ualpha - self->in_Ubeta;
    self->Y = self->in_Ualpha + self->in_Ubeta;
    self->Z = self->in_Ubeta;
    self->W = self->in_Ualpha;

    switch (self->N)
    {
    case SVPWM_2Phase8Vector_Section1:
        self->duty_x = self->X;
        self->duty_y = self->Z;
        break;
    case SVPWM_2Phase8Vector_Section2:
        self->duty_x = -self->X;
        self->duty_y = self->W;
        break;
    case SVPWM_2Phase8Vector_Section3:
        self->duty_x = self->Y;
        self->duty_y = -self->W;
        break;
    case SVPWM_2Phase8Vector_Section4:
        self->duty_x = -self->Y;
        self->duty_y = self->Z;
        break;
    case SVPWM_2Phase8Vector_Section5:
        self->duty_x = -self->X;
        self->duty_y = -self->Z;
        break;
    case SVPWM_2Phase8Vector_Section6:
        self->duty_x = self->X;
        self->duty_y = -self->W;
        break;
    case SVPWM_2Phase8Vector_Section7:
        self->duty_x = -self->Y;
        self->duty_y = self->W;
        break;
    case SVPWM_2Phase8Vector_Section8:
        self->duty_x = self->Y;
        self->duty_y = -self->Z;
        break;
    default:
        break;
    }
    self->duty_0 = (1 - self->duty_x - self->duty_y) / 2;

    /* Over modulation solution */
    float sum_of_duty_x_y;
    sum_of_duty_x_y = self->duty_x + self->duty_y;
    if (sum_of_duty_x_y > 1.0F)
    {
        self->duty_x = self->duty_x / sum_of_duty_x_y;
        self->duty_y = self->duty_y / sum_of_duty_x_y;
    }

    self->duty_a = self->duty_0;
    self->duty_b = self->duty_a + self->duty_x;
    self->duty_c = self->duty_b + self->duty_y;
    // self->duty_a = self->duty_0 / 2;
    // self->duty_b = self->duty_a + self->duty_x / 2;
    // self->duty_c = self->duty_b + self->duty_y / 2;

    switch (self->N)
    {
    case SVPWM_2Phase8Vector_Section1:
        self->duty_cm1 = self->duty_a;
        self->duty_cm2 = self->duty_b;
        self->duty_cm3 = self->duty_c;
        self->duty_cm4 = self->duty_c;
        break;
    case SVPWM_2Phase8Vector_Section2:
        self->duty_cm1 = self->duty_b;
        self->duty_cm2 = self->duty_a;
        self->duty_cm3 = self->duty_c;
        self->duty_cm4 = self->duty_c;
        break;
    case SVPWM_2Phase8Vector_Section3:
        self->duty_cm1 = self->duty_c;
        self->duty_cm2 = self->duty_a;
        self->duty_cm3 = self->duty_b;
        self->duty_cm4 = self->duty_c;
        break;
    case SVPWM_2Phase8Vector_Section4:
        self->duty_cm1 = self->duty_c;
        self->duty_cm2 = self->duty_b;
        self->duty_cm3 = self->duty_a;
        self->duty_cm4 = self->duty_c;
        break;
    case SVPWM_2Phase8Vector_Section5:
        self->duty_cm1 = self->duty_c;
        self->duty_cm2 = self->duty_c;
        self->duty_cm3 = self->duty_a;
        self->duty_cm4 = self->duty_b;
        break;
    case SVPWM_2Phase8Vector_Section6:
        self->duty_cm1 = self->duty_c;
        self->duty_cm2 = self->duty_c;
        self->duty_cm3 = self->duty_b;
        self->duty_cm4 = self->duty_a;
        break;
    case SVPWM_2Phase8Vector_Section7:
        self->duty_cm1 = self->duty_b;
        self->duty_cm2 = self->duty_c;
        self->duty_cm3 = self->duty_c;
        self->duty_cm4 = self->duty_a;
        break;
    case SVPWM_2Phase8Vector_Section8:
        self->duty_cm1 = self->duty_a;
        self->duty_cm2 = self->duty_c;
        self->duty_cm3 = self->duty_c;
        self->duty_cm4 = self->duty_b;
        break;
    default:
        break;
    }

    self->out_Duty_A1 = 1.0F - self->duty_cm1;
    self->out_Duty_A2 = 1.0F - self->duty_cm3;
    self->out_Duty_B1 = 1.0F - self->duty_cm2;
    self->out_Duty_B2 = 1.0F - self->duty_cm4;
    /* **************** */
}

void Chopper_SVPWM_3phase6vector(struct Chopper *self)
{

    // self->OverModulation(self);
}

void Chopper_Output_FastDecay(struct Chopper *self)
{

    /* fast decay */
    if (self->in_Ualpha > 0)
    {
        // if(self->in_Ualpha < self->param_base_pwm)
        // {
        //     self->in_Ualpha = self->param_base_pwm;
        // }
        self->in_Ualpha += self->offset_pwm;
        self->out_Duty_A1 = Limit(fabsf(self->in_Ualpha), 0, 1);
        self->out_Duty_A2 = 0;
    }
    else if (self->in_Ualpha < 0)
    {
        // if(self->in_Ualpha > -self->param_base_pwm)
        // {
        //     self->in_Ualpha = -self->param_base_pwm;
        // }
        self->in_Ualpha -= self->offset_pwm;
        self->out_Duty_A1 = 0;
        self->out_Duty_A2 = Limit(fabsf(self->in_Ualpha), 0, 1);
    }
    else
    {
        self->out_Duty_A1 = 0;
        self->out_Duty_A2 = 0;
    }

    if (self->in_Ubeta > 0)
    {
        // if(self->in_Ubeta < self->param_base_pwm)
        // {
        //     self->in_Ubeta = self->param_base_pwm;
        // }
        self->in_Ubeta += self->offset_pwm;
        self->out_Duty_B1 = Limit(fabsf(self->in_Ubeta), 0, 1);
        self->out_Duty_B2 = 0;
    }
    else if (self->in_Ubeta < 0)
    {
        // if(self->in_Ubeta > -self->param_base_pwm)
        // {
        //     self->in_Ubeta = -self->param_base_pwm;
        // }
        self->in_Ubeta -= self->offset_pwm;
        self->out_Duty_B1 = 0;
        self->out_Duty_B2 = Limit(fabsf(self->in_Ubeta), 0, 1);
    }
    else
    {
        self->out_Duty_B1 = 0;
        self->out_Duty_B2 = 0;
    }
}

void Chopper_Output_SlowDecay(struct Chopper *self)
{
    /* slow decay */
    if (self->in_Ualpha > 0)
    {
        self->in_Ualpha += self->offset_pwm;
        self->out_Duty_A1 = 1;
        self->out_Duty_A2 = Limit(1 - fabsf(self->in_Ualpha), 0, 1);
    }
    else if (self->in_Ualpha < 0)
    {
        self->in_Ualpha -= self->offset_pwm;
        self->out_Duty_A1 = Limit(1 - fabsf(self->in_Ualpha), 0, 1);
        self->out_Duty_A2 = 1;
    }
    else
    {
        self->out_Duty_A1 = 1;
        self->out_Duty_A2 = 1;
    }

    if (self->in_Ubeta > 0)
    {
        self->in_Ubeta += self->offset_pwm;
        self->out_Duty_B1 = 1;
        self->out_Duty_B2 = Limit(1 - fabsf(self->in_Ubeta), 0, 1);
    }
    else if (self->in_Ubeta < 0)
    {
        self->in_Ubeta -= self->offset_pwm;
        self->out_Duty_B1 = Limit(1 - fabsf(self->in_Ubeta), 0, 1);
        self->out_Duty_B2 = 1;
    }
    else
    {
        self->out_Duty_B1 = 1;
        self->out_Duty_B2 = 1;
    }
}

void Chopper_Output_HybridDecay(struct Chopper *self)
{
    // self->in_Ualpha = 0.3;
    // self->in_Ubeta = 0.2;

    // self->SVPWM(self);

    float Ualpha_abs = abs_bm(self->in_Ualpha);
    float Ubeta_abs = abs_bm(self->in_Ubeta);

    self->fast_decay_duty_Aphase = (1 - Ualpha_abs) / (self->param_hybrid_decay_ratio + 1);
    self->slow_decay_duty_Aphase = (1 - Ualpha_abs) - self->fast_decay_duty_Aphase;

    self->fast_decay_duty_Bphase = (1 - Ubeta_abs) / (self->param_hybrid_decay_ratio + 1);
    self->slow_decay_duty_Bphase = (1 - Ubeta_abs) - self->fast_decay_duty_Bphase;

    /* slow decay */
    if (self->in_Ualpha > 0)
    {
        self->in_Ualpha += self->offset_pwm;
        self->out_Duty_A1 = 1 - self->fast_decay_duty_Aphase;
        self->out_Duty_A2 = Limit(1 - Ualpha_abs - self->fast_decay_duty_Aphase, 0, 1);
    }
    else if (self->in_Ualpha < 0)
    {
        self->in_Ualpha -= self->offset_pwm;
        self->out_Duty_A1 = Limit(1 - Ualpha_abs - self->fast_decay_duty_Aphase, 0, 1);
        self->out_Duty_A2 = 1 - self->fast_decay_duty_Aphase;
    }
    else
    {
        self->out_Duty_A1 = 1;
        self->out_Duty_A2 = 1;
    }

    if (self->in_Ubeta > 0)
    {
        self->in_Ubeta += self->offset_pwm;
        self->out_Duty_B1 = 1 - self->fast_decay_duty_Bphase;
        self->out_Duty_B2 = Limit(1 - Ubeta_abs - self->fast_decay_duty_Bphase, 0, 1);
    }
    else if (self->in_Ubeta < 0)
    {
        self->in_Ubeta -= self->offset_pwm;
        self->out_Duty_B1 = Limit(1 - Ubeta_abs - self->fast_decay_duty_Bphase, 0, 1);
        self->out_Duty_B2 = 1 - self->fast_decay_duty_Bphase;
    }
    else
    {
        self->out_Duty_B1 = 1;
        self->out_Duty_B2 = 1;
    }

    /* ****** */

    // DBprintf("***nor***\r\n");
    // DBprintf("%s\r\n", float2str(self->out_Duty_A1, 4));
    // DBprintf("%s\r\n", float2str(self->out_Duty_A2, 4));
    // DBprintf("%s\r\n", float2str(self->out_Duty_B1, 4));
    // DBprintf("%s\r\n", float2str(self->out_Duty_B2, 4));

    // // self->out_Duty_A1 = 1.0F - self->duty_cm1;
    // // self->out_Duty_A2 = 1.0F - self->duty_cm3;
    // // self->out_Duty_B1 = 1.0F - self->duty_cm2;
    // // self->out_Duty_B2 = 1.0F - self->duty_cm4;

    // DBprintf("***sv***\r\n");
    // DBprintf("%d\r\n", self->N);
    // DBprintf("%s\r\n", float2str(self->out_Duty_A1, 4));
    // DBprintf("%s\r\n", float2str(self->out_Duty_A2, 4));
    // DBprintf("%s\r\n", float2str(self->out_Duty_B1, 4));
    // DBprintf("%s\r\n", float2str(self->out_Duty_B2, 4));

    /* ******* */
    // DBprintf("=-=-\r\n");
    // DBprintf("%s\r\n", float2str(self->in_Ualpha, 4));
    // DBprintf("%s\r\n", float2str(self->fast_decay_duty_Aphase, 4));
    // DBprintf("%s\r\n", float2str(self->slow_decay_duty_Aphase, 4));
    // DBprintf("%s\r\n", float2str(self->out_Duty_A1, 4));
    // DBprintf("%s\r\n", float2str(self->out_Duty_A2, 4));

    // DBprintf("%s\r\n", float2str(self->in_Ubeta, 4));
    // DBprintf("%s\r\n", float2str(self->fast_decay_duty_Bphase, 4));
    // DBprintf("%s\r\n", float2str(self->slow_decay_duty_Bphase, 4));
    // DBprintf("%s\r\n", float2str(self->out_Duty_B1, 4));
    // DBprintf("%s\r\n", float2str(self->out_Duty_B2, 4), 4));
}

void Chopper_Output_AdaptiveHybridDecay(struct Chopper *self)
{
}
